// #include "main.h"
#include "../Components/uart/uart.h"
#include "../Components/basic/basic.h"
#include "../Components/algorithm/algorithm.h"
#include <stdio.h>
#include "app/app_motion.h"

extern TIM_HandleTypeDef htim2;

static const char *TAG = "Main";

Motor *motorL = nullptr, *motorR = nullptr;

extern "C"
{
    float target_speed = 0.8;
    void setup()
    {
        serial_init(
            [](uint8_t *data, uint16_t data_len)
            {
                printf("recv data\n");
                printf("Recv:%s \nlen = %d\n", (char *)data, data_len);
                return;
            });

        // motorL = new Motor(&htim2, TIM_CHANNEL_3, MOTOR_L_A_GPIO_Port, MOTOR_L_A_Pin, MOTOR_L_B_Pin, ENC_L_B_GPIO_Port, ENC_L_A_Pin, ENC_L_B_Pin, ENC_L_A_EXTI_IRQn);
        // motorR = new Motor(&htim2, TIM_CHANNEL_4, MOTOR_R_A_GPIO_Port, MOTOR_R_A_Pin, MOTOR_R_B_Pin, ENC_R_B_GPIO_Port, ENC_R_A_Pin, ENC_R_B_Pin, ENC_R_A_EXTI_IRQn, true);

        motorL = new Motor({&htim2, TIM_CHANNEL_3}, {MOTOR_L_A_GPIO_Port, MOTOR_L_A_Pin}, {MOTOR_L_B_GPIO_Port, MOTOR_L_B_Pin}, {ENC_L_A_GPIO_Port, ENC_L_A_Pin}, {ENC_L_B_GPIO_Port, ENC_L_B_Pin}, ENC_L_A_EXTI_IRQn);
        motorR = new Motor({&htim2, TIM_CHANNEL_4}, {MOTOR_R_A_GPIO_Port, MOTOR_R_A_Pin}, {MOTOR_R_B_GPIO_Port, MOTOR_R_B_Pin}, {ENC_R_A_GPIO_Port, ENC_R_A_Pin}, {ENC_R_B_GPIO_Port, ENC_R_B_Pin}, ENC_R_A_EXTI_IRQn, true);

        motorL->set_target_speed(target_speed);
        motorR->set_target_speed(target_speed);

        //  motorR->set_torque(1);
    }

    void loop()
    {
        motorR->poll(0.01);
        printf(",");
        delay(5);
        motorL->poll(0.01);
        printf("\n");
        delay(5);
    }
}
